#include "MEI.h"

MotionEnegry::MotionEnegry(const char * _videoFileName,const char * _featureFileName, int _col, int _row, int _meiLength):videoFileName(_videoFileName),featureFileName(_featureFileName),COL(_col),ROW(_row),MEILength(_meiLength)
{
  ansImage = NULL ;
  nowImage = NULL ;
  if( access(featureFileName.c_str(),F_OK) == 0 )
    {
      INFO2(" MEI feature is exist!",featureFileName) ;
      return ;
    }
  calculateFeature() ;
}
MotionEnegry::~MotionEnegry() 
{
  while(imageList.size())
    {
      IplImage* ptr = imageList.begin()->second ;
      cvReleaseImage(&ptr) ;
      imageList.pop_front() ;
    }
  if( ansImage != NULL )
    cvReleaseImage(&ansImage) ;
  if( nowImage != NULL )
    cvReleaseImage(&nowImage) ;
}
void MotionEnegry::calculateFeature()
{
  std::ofstream fileOutStream(featureFileName.c_str()) ;
  CvCapture* capture = cvCaptureFromFile(videoFileName.c_str()) ;
  if( capture == NULL )
    {
      INFO2("video file name", videoFileName.c_str());
      ERROR("can't open the video") ;
      exit(-1) ;
    }

  while(true)
    {
      if( imageList.size() < MEILength && pushImage(capture) == 0) 
	break ;
      if( imageList.size() < MEILength ) break ;
      float feature[COL*ROW] ;
      float huFeature[7] ;
      int imageNum = getMEI(feature) ;
      calculateSevenHuFeature(feature,huFeature) ;
      fileOutStream << "# " << imageNum << "\n" ;
      for( int i = 0 ; i < 7 ; ++ i )
	fileOutStream << huFeature[i] << "\t" ;
      fileOutStream << "\n" ;
      IplImage* ptr = imageList.begin()->second ;
      cvReleaseImage(&ptr) ;
      imageList.pop_front() ;
    }
}
int MotionEnegry::getMEI(float * _featureBuf)
{
/*
  IplImage * saveImage = cvCreateImage(cvSize(100,100),IPL_DEPTH_8U,1) ;
  IplImage * sourceImage = cvCreateImage(cvSize(10,10),IPL_DEPTH_8U,1) ;
  uchar * sourceData = (uchar*)sourceImage->imageData ;
  uchar * sourcePoint ;
*/
  int imageNum ; 
  assert(imageList.size() >= MEILength ) ; 
  std::list<std::pair<int,IplImage*> >::iterator iter = imageList.begin() ; 
  uchar** imageData = new uchar*[MEILength] ;
  uchar** imagePoint = new uchar*[MEILength] ;
  
  int rows = iter->second->height ;
  int cols = iter->second->width ;
  int widthStep = iter->second->widthStep ;
  int nChannels = iter->second->nChannels; 

  imageNum = iter->first + (MEILength>>1) ;

  for( int index = 0 ; index < MEILength ; ++ index )
    {
      imageData[index] = (uchar*)iter->second->imageData ;
      ++iter ;
    }
  for( int index_row = 0 ; index_row < rows ; ++ index_row )
    {
      //sourcePoint = sourceData ;
      for( int index = 0 ; index < MEILength ; ++ index )
	imagePoint[index] = imageData[index]  ;
      for( int index_col = 0 ; index_col  < cols ; ++ index_col )
	{
	  int sum = 0 ;
	  for( int index = 0 ; index < MEILength ; ++ index )
	    sum += *imagePoint[index] ;
	  _featureBuf[index_row*cols+index_col] = sum > 0 ? 255.f : 0.f ;
	  //*sourcePoint = sum > 0 ? 255:0;
	  for( int index = 0 ; index < MEILength ; ++ index )
	    imagePoint[index] += nChannels ;
	  //sourcePoint += nChannels ;
	}
      //sourceData += widthStep ;
      for( int index = 0 ; index < MEILength ; ++ index )
	imageData[index] += widthStep ;
    }
/*
  cvResize(sourceImage,saveImage) ;
  char imageName[25] ;
  itoa(imageNum,imageName,30) ;
  strcat(imageName,".jpg") ;
  std::string imageFileName = "../PicResult/" ;
  imageFileName += imageName ;
  cvSaveImage(imageFileName.c_str(),saveImage) ;
  cvReleaseImage(&sourceImage) ;
  cvReleaseImage(&saveImage) ;
  */
  delete []imageData ;
  delete []imagePoint ;
  return imageNum ;
}

void MotionEnegry::calculateSevenHuFeature(float* _featureBuf, float * _huFeature)  
{
  assert(_featureBuf!=NULL&&_huFeature!=NULL) ;
  float m00 = calculateM(_featureBuf,0,0) ;
  float m10 = calculateM(_featureBuf,1,0) ; // x col 
  float m01 = calculateM(_featureBuf,0,1) ; // y row
  float avgX = m10 / m00 ;
  float avgY = m01 / m00 ;
  float U[4][4] ;
  for( int p = 0 ; p < 4 ; ++ p )
    for( int q = 0 ; q < 4 ; ++ q )
      U[p][q] = calculateU(_featureBuf,p,q,avgX,avgY) ;

  for( int p = 0 ; p < 4 ; ++ p )
    for( int q = 0 ; q < 4 ; ++ q )
      if( p + q != 0 ) 
	U[p][q] /= pow(U[0][0],float((p+q+2)>>1)) ;
  
  _huFeature[0] = U[2][0] + U[0][2] ;
  _huFeature[1] = (U[2][0]-U[0][2])*(U[2][0]-U[0][2])+4*U[1][1]*U[1][1] ;
  _huFeature[2] = (U[3][0]-3*U[1][2])*(U[3][0]-3*U[1][2]) + (3*U[2][1]-U[0][3])*(3*U[2][1]-U[0][3]) ;
  _huFeature[3] = (U[3][0]+U[1][2])*(U[3][0]+U[1][2]) + (U[2][1]+U[0][3])*(U[2][1]+U[0][3]) ;
  _huFeature[4] = (U[3][0]-3*U[1][2])*(U[3][0]+U[1][2])*((U[3][0]+U[1][2])*(U[3][0]+U[1][2]) - 3 * (U[2][1]+U[0][3]) * (U[2][1]+U[0][3])) + (3*U[2][1]-U[0][3])*(U[2][1]+U[0][3])*(3*(U[3][0]+U[1][2])*(U[3][0]+U[1][2])-(U[2][1]+U[0][3])*(U[2][1]+U[0][3])) ;
  _huFeature[5] = (U[2][0]-U[0][2])*((U[3][0]+U[1][2])*(U[3][0]+U[1][2])-(U[2][1]+U[0][3])*(U[2][1]+U[0][3]))+4*U[1][1]*(U[3][0]+U[1][2])*(U[2][1]+U[0][3]) ;
  _huFeature[6] = (3*U[2][1]-U[0][3])*(U[3][0]+U[1][2])*((U[3][0]+U[1][2])*(U[3][0]+U[1][2])-3*(U[2][1]+U[0][3])*(U[2][1]+U[0][3]))-(U[3][0]-3*U[1][2])*(U[2][1]+U[0][3])*(3*(U[3][0]+U[1][2])*(U[3][0]+U[1][2])-(U[2][1]+U[0][3])*(U[2][1]+U[0][3])) ;
}
// p col 
// q row 
float MotionEnegry::calculateM(float* _featureBuf, int p, int q)
{
  assert(_featureBuf!=NULL) ;
  float retM = 0 ;
  for( int index_row = 0 ; index_row < ROW ; ++ index_row )
    for( int index_col = 0 ; index_col < COL ; ++ index_col )
	retM += _featureBuf[index_row*COL+index_col] * pow(index_col,p) * pow(index_row,q) ;
  return retM ;
}
float MotionEnegry::calculateU(float* _featureBuf, int p, int q, float _avgX, float _avgY)
{
  assert(_featureBuf!=NULL) ;
  float retU = 0 ;
  for( int index_row = 0 ; index_row < ROW ; ++ index_row )
    for( int index_col = 0 ; index_col < COL ; ++ index_col )
      retU += _featureBuf[index_row*COL+index_col] * pow(index_col-_avgX,p) * pow(index_row-_avgY,q) ;
  return retU ;
}
// 一次push 10帧进入队列
unsigned MotionEnegry::pushImage(CvCapture* _capture)
{
  if( _capture == NULL )
    {
      ERROR("capture is NULL!") ;
      exit(-1) ;
    }
  int index ; 
  if( imageList.empty() ) 
    {
      index = 0 ;
    }
  else
    {
      index = imageList.back().first + 1 ;
    }
  int i ; 
  for( i = 0 ; i < 10 ; ++ i )
    {
      IplImage * imagePtr = cvQueryFrame(_capture) ;
      if( ansImage == NULL )
	{
	  index++ ;
	  if( imagePtr->nChannels == 1 )
	    {
	      ansImage = cvCreateImage(cvSize(COL,ROW),IPL_DEPTH_8U,1) ;
	      cvResize(imagePtr,ansImage) ;
	      nowImage = cvCloneImage(imagePtr) ;
	      continue ;
	    }
	  else
	    {
	      ansImage = cvCreateImage(cvSize(COL,ROW),IPL_DEPTH_8U,1);
	      IplImage * tempImage = cvCreateImage(cvGetSize(imagePtr),IPL_DEPTH_8U,1) ;
	      cvCvtColor(imagePtr,tempImage,CV_BGR2GRAY) ;
	      cvResize(tempImage,ansImage) ;
	      nowImage = cvCloneImage(ansImage) ;
	      cvReleaseImage(&tempImage) ;
	      continue ;
	    }
	}
      if( imagePtr == NULL ) break ;
      if( imagePtr->nChannels == 1 )
	cvResize(imagePtr,nowImage) ;
      else
	{
	  IplImage* tempImage = cvCreateImage(cvGetSize(imagePtr),IPL_DEPTH_8U,1) ;
	  cvCvtColor(imagePtr,tempImage,CV_BGR2GRAY) ;
	  cvResize(tempImage,nowImage) ;
	}
      IplImage * diffImage = cvCreateImage(cvSize(10,10),IPL_DEPTH_8U,1) ;
      getDiffImage(ansImage,nowImage,diffImage) ;
      cvCopyImage(nowImage,ansImage) ;
      imageList.push_back(std::make_pair(index++,diffImage)) ;
    }
  return i ;
}
void MotionEnegry::getDiffImage(IplImage* _ansImage, IplImage* _nowImage, IplImage* _diffImage)
{
  assert(_ansImage!=NULL&&_nowImage!=NULL&&_diffImage!=NULL) ;
  
  int rows = _ansImage->height ;
  int cols = _ansImage->width ;
  int nChannels = _ansImage->nChannels ;
  int widthStep = _ansImage->widthStep ;
  
  uchar* ansImageData = (uchar*)_ansImage->imageData ;
  uchar* nowImageData = (uchar*)_nowImage->imageData ;
  uchar* diffImageData = (uchar*)_diffImage->imageData ;
  uchar* ansImagePoint = NULL ;
  uchar* nowImagePoint = NULL ;
  uchar* diffImagePoint = NULL ;
  
  for( int index_row = 0 ; index_row < rows ; ++ index_row )
    {
      ansImagePoint = ansImageData ;
      nowImagePoint = nowImageData ;
      diffImagePoint = diffImageData ;
      for( int index_col = 0 ; index_col < cols ; ++ index_col )
	{
	  if( *ansImagePoint > *nowImagePoint )
	    *diffImagePoint = *ansImagePoint-*nowImagePoint ;
	  else
	    *diffImagePoint = *nowImagePoint-*ansImagePoint ;
	  *diffImagePoint = *diffImagePoint>10?1:0;
	  ansImagePoint += nChannels ;
	  nowImagePoint += nChannels ;
	  diffImagePoint += nChannels ;
	}
      ansImageData += widthStep ;
      nowImageData += widthStep ;
      diffImageData += widthStep ;
    }
}
